/*
 * 	FineAlignmentSM.c
 *
 *	Description: State machine for fine alignment.  
 * 	After the URF sensor is used to coarsely align the 
 * 	platform  the RFID tags mounted to the bottom of 
 * 	the car are used to fine tune alignment. 
 *
 *  Created on: Dec 1, 2012
 *      Author: banfield
 */

/*****************************************************
 * Include Statements
 ****************************************************/
#include "FineAlignmentSM.h"
#include "inc/lm3s2965.h"
#include "MotorControl.h"
#include "MR102A.h"
#include "FineLocalization.h"
#include <stdio.h>

/*****************************************************
 * Defines
 ****************************************************/
#define DEBOUNCE_COUNT 5 // used to prevent false negatives

/*****************************************************
 * Module Level Variables
 ****************************************************/
// Module level variable contains the current state
static FineAlignmentSMState_t currentState;
static char dCount; // a count of negative samples form RFID reader 

/*****************************************************
 * Function: InitFineAlignmentSM
 ****************************************************/
void InitFineAlignmentSM(void)
{
	// All initialization connected to this SM goes here
	currentState = LookingForTag1;
	dCount = 0;
	puts("FineAlignment: Initializing");
}

/*****************************************************
 * Function: RunFineAlignmentSM
 ****************************************************/
char RunFineAlignmentSM(void)
{
	static int theta11; // angle when tag 1 is found
	static int theta12;	// angle when tag 1 is lost
	static int theta21;	// angle when tag 2 is found
	static int theta22;	// angle when tag 2 is lost
	int statusArr[2];	// array of sensor reading

	//int direction = CNTR_CLKWISE;
	//SetDirection(&direction);

	float phi = SingleStepMotor(PHI_MOTOR, CLKWISE);

	// check for tags
	tagEventChecker(statusArr);

	// execute one of the following cases based on the value of currentState 
	switch(currentState)
	{
		case LookingForTag1:
		{
			// if tag 1 is found, record the angle and update the state
			if(statusArr[0] == TAG_FOUND){
				theta11 = phi;
				puts("FineAlignmentSM: Tag found, going to PassingTag1");
				currentState = PassingTag1;
			}
		}
		break;
		case PassingTag1:
		{
			if(statusArr[0] == TAG_LOST){
				
				// To prevent false negatives, there must be DEBOUNCE_COUNT negative
				// sample in a row. If that occurs, the angle of the platform when the 
				// first negative sample was taken is recorded and the state is updated
				dCount++;

				if(dCount == 1){
					puts("Start incrementing dCount.");

					theta12 = phi;
				}else if(dCount >= DEBOUNCE_COUNT){
					puts("FineAlignmentSM: Tag lost, going to LookingForTag2");
					currentState = LookingForTag2;

					dCount = 0;
				}
			}else{
				// for testing
				if(dCount > 0)
					printf("Bouncing occurred. dCount = %i\n", dCount);

				dCount = 0;
			}
		}
		break;
		case LookingForTag2:
		{
			// if tag 2 is found, record the angle and update the state
			if(statusArr[0] == TAG_FOUND){
				theta21 = phi;
				puts("FineAlignmentSM: Tag found, going to PassingTag2");
				currentState = PassingTag2;
			}
		}
		break;
		case PassingTag2:
		{
			if(statusArr[0] == TAG_LOST){
				
				// To prevent false negatives, there must be DEBOUNCE_COUNT negative
				// sample in a row. If that occurs, the angle of the platform when the 
				// first negative sample was taken is recorded and the state is updated
				dCount++;

				if(dCount == 1){
					puts("Start incrementing dCount.");

					theta22 = phi;
				}else if(dCount >= DEBOUNCE_COUNT){
					// When tag 2 is lost, the secondary coil position is calculated and the platform is
					// moved to align the primary coil
				
					//fineLocalize(0, 0, theta11, theta12, theta21, theta22); // for testing
					fineLocalize(GetCurrentX(), GetCurrentY(), theta11, theta12, theta21, theta22);
					puts("FineAlignmentSM: Tag lost, moving to final position!");
					
					MoveToCoord(getXCar(), getYCar(), getPhiCar());

					dCount = 0;

					currentState = LookingForTag1;
					return 1;
				}
			}else{
				// for testing
				if(dCount > 0)
					printf("Bouncing occurred. dCount = %i\n", dCount);

				dCount = 0;
			}
		}
	}

	return 0;
}
